#!  /usr/bin/env python
# coding=utf-8

import rospy
from serial_demo.msg import ti5_polyJC_Lists
import json
import numpy as np
import matplotlib.pyplot as plt
import math

from ti5_fkik import ti5FK,TFMeef,ti5_optIK,ti5SimpleIK_p2j

pi = math.pi


def print_jointRadList(jointTarget):
    print('jointTarget:')
    for i in range(0,len(jointTarget.joint1RadList)):
        print(jointTarget.joint1RadList[i],\
                jointTarget.joint2RadList[i],\
                jointTarget.joint3RadList[i],\
                jointTarget.joint4RadList[i],\
                jointTarget.joint5RadList[i],\
                jointTarget.joint6RadList[i],\
                jointTarget.joint7RadList[i])
    pass

# 从dir.txt获取关节角度
def getJdegForm_txt(dir):
    
    with open(dir, mode='r') as Jfile:
        Jdata = Jfile.readlines()
        groups_jdeg = (len(Jdata)-1)
        Jdeg = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]*groups_jdeg
        print("groups of Jdeg:",(len(Jdata)-1))
        if(Jdata[0]=="polyJdeg\r\n"):# dir.txt first line must be 'polyJdeg'
            print("open polyJdeg!")
            for i in range(0,groups_jdeg):
                Jdata_split = Jdata[1+i].split(',')
                Jdeg[0+(i*7)] = float(Jdata_split[0])
                Jdeg[1+(i*7)] = float(Jdata_split[1])
                Jdeg[2+(i*7)] = float(Jdata_split[2])
                Jdeg[3+(i*7)] = float(Jdata_split[3])
                Jdeg[4+(i*7)] = float(Jdata_split[4])
                Jdeg[5+(i*7)] = float(Jdata_split[5])
                Jdeg[6+(i*7)] = 0
                pass
        else:
            
            print("first line are not polyJdeg ",Jdata[0])
            pass
        pass# with

    return Jdeg
    pass


# 调用此函数只需传入角度值
def do_JList_linear(jointTarget,cJ,nJ):
    #jointxRadList is rad
    jointTarget.joint1RadList = [cJ[0],     nJ[0]]
    jointTarget.joint2RadList = [cJ[1],     nJ[1]]
    jointTarget.joint3RadList = [cJ[2],     nJ[2]]
    jointTarget.joint4RadList = [cJ[3],     nJ[3]]
    jointTarget.joint5RadList = [cJ[4],     nJ[4]]
    jointTarget.joint6RadList = [cJ[5],     nJ[5]]
    return jointTarget

def testMotor(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0
    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 1 # 默认不运行
    
    jointTarget = ti5_polyJC_Lists()

    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 7
    jointTarget.interpolyReq = 1 # linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
   
    # **************************************************************************

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0
  
    # from pose 0 to pose 1
    if(auto_run or pose_step==1):
        #jointxRadList is deg
        cJ = [0,0,0,0,0,0]
        nJ = [-45.0, -18.1997, -67.9756, 0.0, -3.8245, -135.0]
        jointTarget = do_JList_linear(jointTarget,cJ,nJ)
        jointTarget.joint7RadList = [0,     0]
        
        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more:
            rate.sleep()
            rate.sleep()
        
    pass

def JLinerPoly(sJDeg, eJDeg, pub,cmd_rate,use_time):
    """sJDeg,eJDeg : deg
       cmd_rate: Hz
       use_time: s
    """
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 0 # external J space linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(cmd_rate)
    # **************************************************************************
    
    poly_step = int(use_time*cmd_rate)
    #jointxRadList is deg
    sJ = np.array(sJDeg, np.float)
    eJ = np.array(eJDeg, np.float)
    cnJ = np.array(eJ-sJ,np.float)
    stepValue = np.array(cnJ/poly_step,np.float)

    for i in range(1,poly_step+1):
        jointTarget.joint1RadList.append(sJDeg[0] + (i*stepValue[0]))
        jointTarget.joint2RadList.append(sJDeg[1] + (i*stepValue[1]))
        jointTarget.joint3RadList.append(sJDeg[2] + (i*stepValue[2]))
        jointTarget.joint4RadList.append(sJDeg[3] + (i*stepValue[3]))
        jointTarget.joint5RadList.append(sJDeg[4] + (i*stepValue[4]))
        jointTarget.joint6RadList.append(sJDeg[5] + (i*stepValue[5]))
   
    pub.publish(jointTarget)

    print('cmd_rate',cmd_rate, 'cmd_time',use_time)
    print('sJ', sJ)
    print('eJ', eJ)
    print("LinerPoly pub ok")
    
    pass

def testMotor_LinerPoly(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0
    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 1 # 默认不运行
    
    jointTarget = ti5_polyJC_Lists()

    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 10 # iner linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = 500
    # **************************************************************************

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0
  
    # from pose 0 to pose 1
    if(auto_run or pose_step==1):
        #jointxRadList is deg
        # nJ = [  0.0,    0.0,        0.0,      0.0,      0.0,      0.0,      0.0,
        #          0.0,    0.0,   -90,     0.0,    0.0,   0.0,   0.0]
  
        nJ = [  0.0,    0.0,   0.0,     0.0,    0.0,   0.0,   0.0,
                0.0,    0.0,   0.0,     0.0,    0.0,   0.0,   0.0 ]    

        # nJ = [  0.0,    0.0,   -90,     0.0,    0.0,   0.0,   0.0,
        #         0.0,    0.0,   0.0,     0.0,    0.0,   0.0,   0.0 ]
      
        # nJ = [   23.538, -47.797, -63.448,   0.0 ,    18.196, -66.462,  -600000,
        #       0.0,     0.0,    -90,       0.0,     0.0,     0.0,      0.0 ]
        
        # nJ = [  0.0,     0.0,    -90,       0.0,     0.0,     0.0,      0.0 ,  
        #        23.538, -47.797, -63.448,   0.0 ,    21.2445, -66.462,    0.0 ]
        
        # nJ = [ 0.0,     0.0,    0.0,       0.0,     0.0,     0.0,      0.0 ,
        #        -90.0,     0.0,    -90.0,       0.0,     0.0,     0.0,      0.0]
        
        # nJ = [ -90.0,     -60.0,    -30.0,       0.0,     0.0,     0.0,      0.0,
        # 0.0,     0.0,    0.0,       0.0,     0.0,     0.0,      0.0]

        # nJ = [  23.538, -47.797, -63.448,   0.0 ,    21.2445, -66.462,    0.0  ,  
        #        23.5382, -49.0220, -65.0380,   0.0 ,    24.0600, -66.418,    0.0 ]

        # nJ = [23.5382, -49.0220, -65.0380,   0.0 ,    24.0600, -66.418,    0.0  ,  
        #        0.0, 0.0, -90.0,   0.0 ,    0.0, 0.0,    0.0 ]

        # nJ = [  0.0,     0.0,    -90,       0.0,     0.0,     0.0,      0.0,
        #         23.538, -47.797, -63.448,   0.0 ,    18.196, -66.462,  -600000 ]
        
        # nJ = [  23.538, -47.797, -63.448,   0.0 ,    18.196, -66.462,  -600000,
        #       -19.654 , -46.186,  -66.754 ,   0.0 ,   20.155 ,  -109.654,   -600000 ]
        
        # nJ = [  -19.654 , -46.186,  -66.754 ,   0.0 ,   20.155 ,  -109.654,   -600000,
        #       0.0,    0.0,        -90.0,      0.0,      0.0,      0.0,      -600000 ]
       

        # nJ = [  -45.0, -18.1997, -67.9756,    0.0,     -3.8245,   -135.0,   0.0,
        #         -14.64, -28.68, -33.04,    -18.13,     -32.45,   0.0,   0.0]

        # nJ = [  -45.0, -18.1997, -67.9756,    0.0,     -3.8245,   -135.0,   0.0,
        #         45.0,  -18.1997, -67.9756,    0.0,     -3.8245,   -45.0,    0.0]

        # nJ = [  -45.0, -18.1997, -67.9756,    0.0,     -3.8245,   -135.0,   0.0,
        #         0.0,    0, 0,    0.0,        0.0,      0.0,       0.0,       0.0  ]

        # nJ = [  45.0,  -18.1997, -67.9756,    0.0,     -3.8245,   -45.0,    0.0,
        #         0.0,    0.0,      0.0,        0.0,      0.0,       0.0,   0.0]
        
        # nJ = [  45.0,  -18.1997, -67.9756,    0.0,     -3.8245,   -45.0,    0.0,
        #        -45.0, -18.1997, -67.9756,    0.0,     -3.8245,   -135.0,   0.0]
        
        for i in range(0,len(nJ)/7):
            jointTarget.joint1RadList.append(nJ[0+i*7])
            jointTarget.joint2RadList.append(nJ[1+i*7])
            jointTarget.joint3RadList.append(nJ[2+i*7])
            jointTarget.joint4RadList.append(nJ[3+i*7])
            jointTarget.joint5RadList.append(nJ[4+i*7])
            jointTarget.joint6RadList.append(nJ[5+i*7])
            jointTarget.joint7RadList.append(nJ[6+i*7])

        print_jointRadList(jointTarget)

        jointTarget.timeLists = [0, 6]
        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)

        print("ok")
        rate.sleep()
   
    pass
 
def testMotor_cubic(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0
    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 1 # 默认不运行
    
    jointTarget = ti5_polyJC_Lists()

    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 7
    jointTarget.interpolyReq = 0 # not need interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
   
    # **************************************************************************

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0
  
    # from pose 0 to pose 1
    if(auto_run or pose_step==1):
        #jointxRadList is deg
        nJ = [0,0,0,0,0,0,0]*30000
        deg = 0.003
        for i in range(0,len(nJ)/7):
            nJ[4+i*7] += deg
            deg += 0.003
        
        for i in range(0,len(nJ)/7):
            jointTarget.joint1RadList.append(nJ[0+i*7])
            jointTarget.joint2RadList.append(nJ[1+i*7])
            jointTarget.joint3RadList.append(nJ[2+i*7])
            jointTarget.joint4RadList.append(nJ[3+i*7])
            jointTarget.joint5RadList.append(nJ[4+i*7])
            jointTarget.joint6RadList.append(nJ[5+i*7])
            jointTarget.joint7RadList.append(nJ[6+i*7])
        print('jointTarget:')
        for i in range(0,len(jointTarget.joint1RadList)):
            print(jointTarget.joint1RadList[i],\
                  jointTarget.joint2RadList[i],\
                  jointTarget.joint3RadList[i],\
                  jointTarget.joint4RadList[i],\
                  jointTarget.joint5RadList[i],\
                  jointTarget.joint6RadList[i],\
                  jointTarget.joint7RadList[i])

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more:
            rate.sleep()
            rate.sleep()
        
    pass

def move_path(pub,dir):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0
    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 1 # 默认不运行
    
    jointTarget = ti5_polyJC_Lists()

    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 7
    jointTarget.interpolyReq = 0 # not need interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = 200
    # **************************************************************************

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0
  
    # run path 1
    if(auto_run or pose_step==1):
        #jointxRadList is deg
        nJ = getJdegForm_txt(dir)
        
        
        for i in range(0,len(nJ)/7):
            jointTarget.joint1RadList.append(nJ[0+i*7])
            jointTarget.joint2RadList.append(nJ[1+i*7])
            jointTarget.joint3RadList.append(nJ[2+i*7])
            jointTarget.joint4RadList.append(nJ[3+i*7])
            jointTarget.joint5RadList.append(nJ[4+i*7])
            jointTarget.joint6RadList.append(nJ[5+i*7])
            jointTarget.joint7RadList.append(nJ[6+i*7])
        print('jointTarget:')
        for i in range(0,len(jointTarget.joint1RadList)):
            print(jointTarget.joint1RadList[i],\
                  jointTarget.joint2RadList[i],\
                  jointTarget.joint3RadList[i],\
                  jointTarget.joint4RadList[i],\
                  jointTarget.joint5RadList[i],\
                  jointTarget.joint6RadList[i],\
                  jointTarget.joint7RadList[i])

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more:
            rate.sleep()
            rate.sleep()
    
    pass


def plot(dir):
    with open(dir, mode='r') as Jfile:
        Jdata = Jfile.readlines()
        pass

    groups_jdeg = (len(Jdata)-1)
    Jdeg = np.zeros((groups_jdeg, 6), dtype=np.float) # type:np.ndarray
    
    try:
        head = json.loads(Jdata[0])
        name = head['name']
        cmdTime = head['cmdTime']
        cmdRate = head['cmdRate']
        step = head['step']
    except Exception as e:
        print('json loads msg_head failed!')
        print(e)
        name = ''

    if(name=="JdegLists"):# 
        print("open file!")
        print("groups of Jdeg:",groups_jdeg)
        for i in range(0,groups_jdeg):
            Jdata_split = Jdata[1+i].split(', ')
            Jdeg[i,0] = float(Jdata_split[0])
            Jdeg[i,1] = float(Jdata_split[1])
            Jdeg[i,2] = float(Jdata_split[2])
            Jdeg[i,3] = float(Jdata_split[3])
            Jdeg[i,4] = float(Jdata_split[4])
            Jdeg[i,5] = float(Jdata_split[5])
            pass
        print('Jdeg.shape',Jdeg.shape)

        t = np.linspace(0,cmdTime, Jdeg.shape[0])
        print(t.shape)

        for i in range(0,6):
            plt.figure()
            plt.subplot(4,1,1)
            plt.scatter(t,Jdeg[:,i], s=2)
           
            f1 = np.poly1d(np.polyfit(t,Jdeg[:,i],deg=5))
            df1 = f1.deriv()
            ddf1 = df1.deriv()

            plt.subplot(4,1,2)
            plt.scatter(t,f1(t), s=2)
            plt.subplot(4,1,3)
            plt.scatter(t,df1(t), s=2)
            plt.subplot(4,1,4)
            plt.scatter(t,ddf1(t), s=2)
            pass
        # FK xyz by read joint
        print('FK xyz by read joint')
        xyz = np.zeros((groups_jdeg,3), dtype=np.float)
        for i in range(0,groups_jdeg):
            xyz[i,:] = (ti5FK(Jdeg[i,:]*(pi/180)))[0:3,3]

        for i in range(0,3):
            plt.figure()
            plt.subplot(4,1,1)
            plt.scatter(t,xyz[:,i], s=2)

            f1 = np.poly1d(np.polyfit(t,xyz[:,i],deg=5))
            df1 = f1.deriv()
            ddf1 = df1.deriv()

            plt.subplot(4,1,2)
            plt.scatter(t,f1(t), s=2)
            plt.subplot(4,1,3)
            plt.scatter(t,df1(t), s=2)
            plt.subplot(4,1,4)
            plt.scatter(t,ddf1(t), s=2)
            pass
        
        pass # if(name=="JdegLists")
    else:
        print("name are not JdegLists ",Jdata[0])
        pass

    # read and plot polyPoseLists***********************************
    print('read and plot polyPoseLists')
    dirP = '/home/wheeltec/Desktop/ti5Poly_writePLists.txt'
    with open(dirP, mode='r') as Pfile:
        Pdata = Pfile.readlines()
        pass
    groups_pose = (len(Pdata)-1)
    Pose = np.zeros((groups_pose, 6), dtype=np.float)
    try:
        head = json.loads(Pdata[0])
        nameP = head['name']
        cmdTime = head['cmdTime']
        cmdRate = head['cmdRate']
        step = head['step']
    except Exception as e:
        print('json loads msg_head failed!')
        print(e)
        nameP = ''
        pass # try
    if(nameP=="polyPoseLists"):# 
        print("open file!")
        print("groups of polyPoseLists:",groups_pose)
        for i in range(0,groups_pose):
            Pdata_split = Pdata[1+i].split(', ')
            Pose[i,0] = float(Pdata_split[0])
            Pose[i,1] = float(Pdata_split[1])
            Pose[i,2] = float(Pdata_split[2])
            Pose[i,3] = float(Pdata_split[3])
            Pose[i,4] = float(Pdata_split[4])
            Pose[i,5] = float(Pdata_split[5])
            pass
        print('Pose.shape',Pose.shape)

        t = np.linspace(0,cmdTime, Pose.shape[0])
        print(t.shape)

       
        
        for i in range(0,3):
            plt.figure()
            plt.subplot(4,1,1)
            plt.scatter(t,Pose[:,i], s=2)

            f1 = np.poly1d(np.polyfit(t,xyz[:,i],deg=1))
            df1 = f1.deriv()
            ddf1 = df1.deriv()

            plt.subplot(4,1,2)
            plt.scatter(t,f1(t), s=2)
            plt.subplot(4,1,3)
            plt.scatter(t,df1(t), s=2)
            plt.subplot(4,1,4)
            plt.scatter(t,ddf1(t), s=2)
            pass
        
        pass # if(name=="polyPoseLists")
    else:
        print("name are not polyPoseLists ",Pdata[0])
        pass
     
    plt.show()
    pass

def openGripper(pub, pose):
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 1
    jointTarget.interpolyReq = 10 # iner linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(500)
    # **************************************************************************
    print("*********+++++openGripper start+++*********")
    nJ = [  0.0,    0.0,   -90,     0.0,    0.0,   0.0,  float(pose)]
    for i in range(0,len(nJ)/7):
        jointTarget.joint1RadList.append(nJ[0+i*7])
        jointTarget.joint2RadList.append(nJ[1+i*7])
        jointTarget.joint3RadList.append(nJ[2+i*7])
        jointTarget.joint4RadList.append(nJ[3+i*7])
        jointTarget.joint5RadList.append(nJ[4+i*7])
        jointTarget.joint6RadList.append(nJ[5+i*7])
        jointTarget.joint7RadList.append(nJ[6+i*7])

        jointTarget.timeLists = [0, 4]
    
        pub.publish(jointTarget)
        print("openGripper pub ok")


    pass

# sigmoid 加减速测试，从dir读取关节角度
def sigmoidTestTxt(dir):
    "从dir读取关节角度并返回"
    
    try:
        with open(dir,mode='r') as jfile:
            jdata = jfile.readlines()
            pass
    except Exception as e:
        print(e)
        print('open file {} failed!'.format(dir))
        return
    print('oopen file')

    groups_jdeg = len(jdata) - 1
    jdeg = np.zeros((groups_jdeg,6),dtype=np.float)
    print('groups_jdeg',groups_jdeg)

    for i in range(groups_jdeg):
        jdata_split = jdata[i+1].split(',')
        jdeg[i,0] = float(jdata_split[0])
        jdeg[i,1] = float(jdata_split[1])
        jdeg[i,2] = float(jdata_split[2])
        jdeg[i,3] = float(jdata_split[3])
        jdeg[i,4] = float(jdata_split[4])
        jdeg[i,5] = float(jdata_split[5])
        pass


    return jdeg

    pass

def JPolyFromTxt(pub,poly_rate,poly_step,dir):
    
    
    jList = sigmoidTestTxt(dir)
    try:
        row,cul = jList.shape
    except Exception as e:
        print(e)
        print('get jList.shape failed!')
        return

    if row!=poly_step or cul!=6:
        print('err jList.shape is not (poly_step,6)')
        return
    print('jList.shape',jList.shape)


    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 0 # external J space linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(poly_rate)
    # **************************************************************************
    

    
    #jointxRadList is deg

    for i in range(poly_step):
        jointTarget.joint1RadList.append(jList[i,0])
        jointTarget.joint2RadList.append(jList[i,1])
        jointTarget.joint3RadList.append(jList[i,2])
        jointTarget.joint4RadList.append(jList[i,3])
        jointTarget.joint5RadList.append(jList[i,4])
        jointTarget.joint6RadList.append(jList[i,5])
   
    pub.publish(jointTarget)

    # print(jointTarget.joint1RadList[0:100])
    # print(jointTarget.joint2RadList[0:100])
    # print(jointTarget.joint3RadList[0:100])
    # print(jointTarget.joint4RadList[0:100])
    # print(jointTarget.joint5RadList[0:100])
    # print(jointTarget.joint6RadList[0:100])

    print('cmd_rate',poly_rate, 'cmd_time',float(poly_step)/poly_rate)
    print("jointPoly pub ok")
    
    pass

def quartic_test(pub,mode):

    if mode == 'joint':
        # 关节空间 tcost:3.19081952238691s
        JPolyFromTxt(pub=pub,
                    poly_rate=313,
                    poly_step=1000,
                    dir='/home/wheeltec/wheeltec_arm/txtfile/quartic_TimePolyJdeg.txt')
    elif mode == 'pose':
        # 笛卡尔空间规划 tcost:5.16666666s
        JPolyFromTxt(pub=pub,
                    poly_rate=193,
                    poly_step=1000,
                    dir='/home/wheeltec/wheeltec_arm/txtfile/quartic_DVpolyJdeg.txt')
    
    pass

def move_j(sJDeg, eJDeg, pub,cmd_rate,use_time):
    """sJDeg,eJDeg : deg
       cmd_rate: Hz
       use_time: s
    """
    JLinerPoly(sJDeg, eJDeg, pub,cmd_rate,use_time)
    pass

# 不建议使用
def move_p(sPose,ePose,pub,cmd_rate,use_time,cJdeg):
    """
    sPose:xyz_eulZYX/mm_deg
    """

    poly1_step = int(use_time*cmd_rate)
    sP = np.array(sPose,np.float)
    eP = np.array(ePose,np.float)

    sJ = ti5SimpleIK_p2j(cJdeg, sP)
    eJ = ti5SimpleIK_p2j(sJ, eP)

    cnJ = np.array(eJ-sJ,np.float)
    step_value = np.array(cnJ/poly1_step,np.float)

    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 0 # external J space linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(cmd_rate)
    # **************************************************************************
    
    for i in range(1,poly1_step+1):
        jointTarget.joint1RadList.append(sJ[0] + (i*step_value[0]))
        jointTarget.joint2RadList.append(sJ[1] + (i*step_value[1]))
        jointTarget.joint3RadList.append(sJ[2] + (i*step_value[2]))
        jointTarget.joint4RadList.append(sJ[3] + (i*step_value[3]))
        jointTarget.joint5RadList.append(sJ[4] + (i*step_value[4]))
        jointTarget.joint6RadList.append(sJ[5] + (i*step_value[5]))
        pass
    
    pub.publish(jointTarget)
    print('cmd_rate',cmd_rate, 'cmd_time',use_time)
    print('sP', sP)
    print('sJ', sJ)
    print('eP', eP)
    print('eJ', eJ)
    print("LinerPoly pub ok")

    pass


from ti5GrabFruits import openGripper

if  __name__== "__main__":
    rospy.init_node("interPolyPoint_Pub_node")
    pub = rospy.Publisher("interPolyPoint_topic",ti5_polyJC_Lists,queue_size=1000)
 
    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()#waiter for system

    # rate.sleep()
    # rate.sleep()
    # rate.sleep()
    # rate.sleep()
    # rate.sleep()
    # openGripper(pub = pub, pose = -3000.0)

    JLinerPoly(sJDeg = [ 0.0000, 0.0000, 0.0000, 0.00, 0.0000, 0.0000],
               eJDeg = [ 0.0000, 0.0000, 0.0000, 0.00, 0.0000, 0.0000],
               pub=pub,
               cmd_rate=250,
               use_time=6.0)
    # openGripper(pub = pub, pose = -600000.0)


    # JLinerPoly(sJDeg = [-82.8749, -32.1332, -83.2761, 0.00, 25.4094, 0.0000],
    #            eJDeg = [ -82.8749, -34.8869, -86.9188, 0.00, 31.8058, 0.0000],
    #            pub=pub,
    #            cmd_rate=250,
    #            use_time=6.0)
    # openGripper(pub = pub, pose = 400000.0)
    # openGripper(pub = pub, pose = 30000.0)
    # JLinerPoly(sJDeg = [-82.8749, -34.8869, -86.9188, 0.00, 31.8058, 0.0000],
    #            eJDeg = [ -90.0000, 0.0000, -90.0000, 0.00, 0.0000, 0.0000],
    #            pub=pub,
    #            cmd_rate=250,
    #            use_time=6.0)


    # JLinerPoly(sJDeg = [45.001088003596706, 2.4590922252744867, -92.38874140207422, 0.0, -0.07035082320026459, -44.9989119964033],
    #            eJDeg = [135.0, 2.4583222441659855, -92.38801546326877, 0.0, -0.07030678089722485, 45.0],
    #            pub=pub,
    #            cmd_rate=500,
    #            use_time=6.0)

    ## mode= 'joint' /'pose'
    # rate.sleep()
    # rate.sleep()#waiter for system
    # rate.sleep()
    # rate.sleep()#waiter for system
    # rate.sleep()
    # rate.sleep()#waiter for system

    # quartic_test(pub,mode='pose')


    # 关节空间规划
    # JPolyFromTxt(pub=pub,poly_rate=370,poly_step=1000,dir='/home/wheeltec/wheeltec_arm/txtfile/Jdeg_from_Spoly.txt')
    
    # 笛卡尔空间规划  t=5.166666666
    # JPolyFromTxt(pub=pub,poly_rate=193,poly_step=1000,dir='/home/wheeltec/wheeltec_arm/txtfile/Jdeg_from_DVsync_poly_D1_7.txt')
  
    # testMotor_cubic(pub)


    # JLinerPoly(sJDeg = [90.0000, 0.000, -90.000, 0.00, 0.0000, 0.0000],
    #            eJDeg = [0.0000, 0.000, -90.000, 0.00, 0.0000, -90.0000],
    #            pub=pub,
    #            cmd_rate=500,
    #            use_time=6.0)

    
    # move_path(pub, dir='/home/wheeltec/Desktop/linearPolyJdegList.txt')
 
    # openGripper(pub, -600000.0)

    
    
    # dir = '/home/wheeltec/Desktop/ti5IK_writeJdegList.txt'
    # plot(dir)

    # xyz=ti5FK(np.array([0, 0, 0, 0.0000, 0,0])*(pi/180))
    # print(xyz)
 

  